//
#include "FOC_Core_SL_SGSN_M4A1.h"
//
#define SIN_MASK    0x0300
//
#define AG0_90      0x0200
#define AG90_180    0x0300
#define AG180_270   0x0000
#define AG270_360   0x0100
//
static const SHORT SinCosTable[256] = {\
0x0000,0x00C9,0x0192,0x025B,0x0324,0x03ED,0x04B6,0x057F,\
0x0648,0x0711,0x07D9,0x08A2,0x096A,0x0A33,0x0AFB,0x0BC4,\
0x0C8C,0x0D54,0x0E1C,0x0EE3,0x0FAB,0x1072,0x113A,0x1201,\
0x12C8,0x138F,0x1455,0x151C,0x15E2,0x16A8,0x176E,0x1833,\
0x18F9,0x19BE,0x1A82,0x1B47,0x1C0B,0x1CCF,0x1D93,0x1E57,\
0x1F1A,0x1FDD,0x209F,0x2161,0x2223,0x22E5,0x23A6,0x2467,\
0x2528,0x25E8,0x26A8,0x2767,0x2826,0x28E5,0x29A3,0x2A61,\
0x2B1F,0x2BDC,0x2C99,0x2D55,0x2E11,0x2ECC,0x2F87,0x3041,\
0x30FB,0x31B5,0x326E,0x3326,0x33DF,0x3496,0x354D,0x3604,\
0x36BA,0x376F,0x3824,0x38D9,0x398C,0x3A40,0x3AF2,0x3BA5,\
0x3C56,0x3D07,0x3DB8,0x3E68,0x3F17,0x3FC5,0x4073,0x4121,\
0x41CE,0x427A,0x4325,0x43D0,0x447A,0x4524,0x45CD,0x4675,\
0x471C,0x47C3,0x4869,0x490F,0x49B4,0x4A58,0x4AFB,0x4B9D,\
0x4C3F,0x4CE0,0x4D81,0x4E20,0x4EBF,0x4F5D,0x4FFB,0x5097,\
0x5133,0x51CE,0x5268,0x5302,0x539B,0x5432,0x54C9,0x5560,\
0x55F5,0x568A,0x571D,0x57B0,0x5842,0x58D3,0x5964,0x59F3,\
0x5A82,0x5B0F,0x5B9C,0x5C28,0x5CB3,0x5D3E,0x5DC7,0x5E4F,\
0x5ED7,0x5F5D,0x5FE3,0x6068,0x60EB,0x616E,0x61F0,0x6271,\
0x62F1,0x6370,0x63EE,0x646C,0x64E8,0x6563,0x65DD,0x6656,\
0x66CF,0x6746,0x67BC,0x6832,0x68A6,0x6919,0x698B,0x69FD,\
0x6A6D,0x6ADC,0x6B4A,0x6BB7,0x6C23,0x6C8E,0x6CF8,0x6D61,\
0x6DC9,0x6E30,0x6E96,0x6EFB,0x6F5E,0x6FC1,0x7022,0x7083,\
0x70E2,0x7140,0x719D,0x71F9,0x7254,0x72AE,0x7307,0x735E,\
0x73B5,0x740A,0x745F,0x74B2,0x7504,0x7555,0x75A5,0x75F3,\
0x7641,0x768D,0x76D8,0x7722,0x776B,0x77B3,0x77FA,0x783F,\
0x7884,0x78C7,0x7909,0x794A,0x7989,0x79C8,0x7A05,0x7A41,\
0x7A7C,0x7AB6,0x7AEE,0x7B26,0x7B5C,0x7B91,0x7BC5,0x7BF8,\
0x7C29,0x7C59,0x7C88,0x7CB6,0x7CE3,0x7D0E,0x7D39,0x7D62,\
0x7D89,0x7DB0,0x7DD5,0x7DFA,0x7E1D,0x7E3E,0x7E5F,0x7E7E,\
0x7E9C,0x7EB9,0x7ED5,0x7EEF,0x7F09,0x7F21,0x7F37,0x7F4D,\
0x7F61,0x7F74,0x7F86,0x7F97,0x7FA6,0x7FB4,0x7FC1,0x7FCD,\
0x7FD8,0x7FE1,0x7FE9,0x7FF0,0x7FF5,0x7FF9,0x7FFD,0x7FFE};
//
//*****************************************************************************
//!\brief   Insert measurement vector
//!\param   None
//!\retval  None
//*****************************************************************************
void InsertVector(void)
{
    switch(SVPWM.Sector)
    {
        case SECTOR_1:
            switch(SVPWM.TrigType)
            {
                case TYPE_2:
                    switch(g_PWM_InverseIndex)
                    {
                        case 0:
                            OC1_Mode_ForcedAction_InActive;
                            g_PWM_InverseIndex = 1;
                            break;
                        case 1:
                            OC1_Mode_PWM1;
                            g_PWM_InverseIndex = 2;
                           break;
                        default:
                            break;
                    }
                    //
                    break;
                    //
                case TYPE_3:
                    switch(g_PWM_InverseIndex)
                    {
                        case 0:
                            OC2_Mode_ForcedAction_InActive;
                            g_PWM_InverseIndex = 1;
                            break;
                        case 1:
                            OC2_Mode_PWM1;
                            g_PWM_InverseIndex = 2;
                           break;
                        default:
                            break;
                    }
                    //
                    break;
                    //
                case TYPE_4:
                    switch(g_PWM_InverseIndex)
                    {
                        case 0:
                            OC1_Mode_ForcedAction_InActive;
                            g_PWM_InverseIndex = 1;
                            break;
                        case 1:
                            OC3_Mode_ForcedAction_InActive;
                            g_PWM_InverseIndex = 2;
                            break;
                        case 2:
                            OC3_Mode_PWM1;
                            g_PWM_InverseIndex = 3;
                            break;
                        case 3:
                            OC1_Mode_PWM1;
                            g_PWM_InverseIndex = 4;
                            break;
                        default:
                            break;
                    }
                    //
                    break;
                    //
                default:
                    break;
            }
            //
            break;
            //
        case SECTOR_2:
             switch(SVPWM.TrigType)
            {
                case TYPE_2:
                    switch(g_PWM_InverseIndex)
                    {
                        case 0:
                            OC2_Mode_ForcedAction_InActive;
                            g_PWM_InverseIndex = 1;
                            break;
                        case 1:
                            OC2_Mode_PWM1;
                            g_PWM_InverseIndex = 2;
                           break;
                        default:
                            break;
                    }
                    //
                    break;
                    //
                case TYPE_3:
                    switch(g_PWM_InverseIndex)
                    {
                        case 0:
                            OC1_Mode_ForcedAction_InActive;
                            g_PWM_InverseIndex = 1;
                            break;
                        case 1:
                            OC1_Mode_PWM1;
                            g_PWM_InverseIndex = 2;
                           break;
                        default:
                            break;
                    }
                    //
                    break;
                    //
                case TYPE_4:
                    switch(g_PWM_InverseIndex)
                    {
                        case 0:
                            OC2_Mode_ForcedAction_InActive;
                            g_PWM_InverseIndex = 1;
                            break;
                        case 1:
                            OC3_Mode_ForcedAction_InActive;
                            g_PWM_InverseIndex = 2;
                            break;
                        case 2:
                            OC3_Mode_PWM1;
                            g_PWM_InverseIndex = 3;
                            break;
                        case 3:
                            OC2_Mode_PWM1;
                            g_PWM_InverseIndex = 4;
                            break;
                        default:
                            break;
                    }
                    //
                    break;
                    //
                default:
                    break;
            }
            //
            break;
            //
        case SECTOR_3:
            switch(SVPWM.TrigType)
            {
                case TYPE_2:
                    switch(g_PWM_InverseIndex)
                    {
                        case 0:
                            OC2_Mode_ForcedAction_InActive;
                            g_PWM_InverseIndex = 1;
                            break;
                        case 1:
                            OC2_Mode_PWM1;
                            g_PWM_InverseIndex = 2;
                           break;
                        default:
                            break;
                    }
                    //
                    break;
                    //
                case TYPE_3:
                    switch(g_PWM_InverseIndex)
                    {
                        case 0:
                            OC3_Mode_ForcedAction_InActive;
                            g_PWM_InverseIndex = 1;
                            break;
                        case 1:
                            OC3_Mode_PWM1;
                            g_PWM_InverseIndex = 2;
                           break;
                        default:
                            break;
                    }
                    //
                    break;
                    //
                case TYPE_4:
                    switch(g_PWM_InverseIndex)
                    {
                        case 0:
                            OC2_Mode_ForcedAction_InActive;
                            g_PWM_InverseIndex = 1;
                            break;
                        case 1:
                            OC1_Mode_ForcedAction_InActive;
                            g_PWM_InverseIndex = 2;
                            break;
                        case 2:
                            OC1_Mode_PWM1;
                            g_PWM_InverseIndex = 3;
                            break;
                        case 3:
                            OC2_Mode_PWM1;
                            g_PWM_InverseIndex = 4;
                            break;
                        default:
                            break;
                    }
                    //
                    break;
                    //
                default:
                    break;
            }
            //
            break;
            //
        case SECTOR_4:
            switch(SVPWM.TrigType)
            {
                case TYPE_2:
                    switch(g_PWM_InverseIndex)
                    {
                        case 0:
                            OC3_Mode_ForcedAction_InActive;
                            g_PWM_InverseIndex = 1;
                            break;
                        case 1:
                            OC3_Mode_PWM1;
                            g_PWM_InverseIndex = 2;
                           break;
                        default:
                            break;
                    }
                    //
                    break;
                    //
                case TYPE_3:
                    switch(g_PWM_InverseIndex)
                    {
                        case 0:
                            OC2_Mode_ForcedAction_InActive;
                            g_PWM_InverseIndex = 1;
                            break;
                        case 1:
                            OC2_Mode_PWM1;
                            g_PWM_InverseIndex = 2;
                           break;
                        default:
                            break;
                    }
                    //
                    break;
                    //
                case TYPE_4:
                    switch(g_PWM_InverseIndex)
                    {
                        case 0:
                            OC3_Mode_ForcedAction_InActive;
                            g_PWM_InverseIndex = 1;
                            break;
                        case 1:
                            OC1_Mode_ForcedAction_InActive;
                            g_PWM_InverseIndex = 2;
                            break;
                        case 2:
                            OC1_Mode_PWM1;
                            g_PWM_InverseIndex = 3;
                            break;
                        case 3:
                            OC3_Mode_PWM1;
                            g_PWM_InverseIndex = 4;
                            break;
                        default:
                            break;
                    }
                    //
                    break;
                    //
                default:
                    break;
            }
            //
            break;
            //
        case SECTOR_5:
            switch(SVPWM.TrigType)
            {
                case TYPE_2:
                    switch(g_PWM_InverseIndex)
                    {
                        case 0:
                            OC3_Mode_ForcedAction_InActive;
                            g_PWM_InverseIndex = 1;
                            break;
                        case 1:
                            OC3_Mode_PWM1;
                            g_PWM_InverseIndex = 2;
                           break;
                        default:
                            break;
                    }
                    //
                    break;
                    //
                case TYPE_3:
                    switch(g_PWM_InverseIndex)
                    {
                        case 0:
                            OC1_Mode_ForcedAction_InActive;
                            g_PWM_InverseIndex = 1;
                            break;
                        case 1:
                            OC1_Mode_PWM1;
                            g_PWM_InverseIndex = 2;
                           break;
                        default:
                            break;
                    }
                    //
                    break;
                    //
                case TYPE_4:
                    switch(g_PWM_InverseIndex)
                    {
                        case 0:
                            OC3_Mode_ForcedAction_InActive;
                            g_PWM_InverseIndex = 1;
                            break;
                        case 1:
                            OC2_Mode_ForcedAction_InActive;
                            g_PWM_InverseIndex = 2;
                            break;
                        case 2:
                            OC2_Mode_PWM1;
                            g_PWM_InverseIndex = 3;
                            break;
                        case 3:
                            OC3_Mode_PWM1;
                            g_PWM_InverseIndex = 4;
                            break;
                        default:
                            break;
                    }
                    //
                    break;
                    //
                default:
                    break;
            }
            //
            break;
            //
        case SECTOR_6:
            switch(SVPWM.TrigType)
            {
                case TYPE_2:
                    switch(g_PWM_InverseIndex)
                    {
                        case 0:
                            OC1_Mode_ForcedAction_InActive;
                            g_PWM_InverseIndex = 1;
                            break;
                        case 1:
                            OC1_Mode_PWM1;
                            g_PWM_InverseIndex = 2;
                           break;
                        default:
                            break;
                    }
                    //
                    break;
                    //
                case TYPE_3:
                    switch(g_PWM_InverseIndex)
                    {
                        case 0:
                            OC3_Mode_ForcedAction_InActive;
                            g_PWM_InverseIndex = 1;
                            break;
                        case 1:
                            OC3_Mode_PWM1;
                            g_PWM_InverseIndex = 2;
                           break;
                        default:
                            break;
                    }
                    //
                    break;
                    //
                case TYPE_4:
                    switch(g_PWM_InverseIndex)
                    {
                        case 0:
                            OC1_Mode_ForcedAction_InActive;
                            g_PWM_InverseIndex = 1;
                            break;
                        case 1:
                            OC2_Mode_ForcedAction_InActive;
                            g_PWM_InverseIndex = 2;
                            break;
                        case 2:
                            OC2_Mode_PWM1;
                            g_PWM_InverseIndex = 3;
                            break;
                        case 3:
                            OC1_Mode_PWM1;
                            g_PWM_InverseIndex = 4;
                            break;
                        default:
                            break;
                    }
                    //
                    break;
                    //
                default:
                    break;
            }
            //
            break;
            //
        default:
            break;
    }
}

//*****************************************************************************
//!\brief   Convert the phase current 0 and 1 to Q15 format
//!\param   Phase current 0 and 1,IBus current offset
//!\retval  Current A and B
//*****************************************************************************
T_CurrComps  GetCurr_SingleShunt(SHORT CurrIBus_0,SHORT CurrIBus_1,LONG CurrIBus_Offset,uCHAR OffsetNum)
{T_CurrComps CurrOutput = {0};
 LONG CurrTemp[2] = {0};
 SHORT  CurrIBus_A = 0,CurrIBus_B = 0,CurrIBus_C = 0;               // the phase current A/B/C
 //
    CurrTemp[0] = ((LONG)(CurrIBus_0 * OffsetNum)) - CurrIBus_Offset;
    if(CurrTemp[0] <= SHORT_MIN)
    {
        CurrTemp[0] = SHORT_MIN;
    }
    else if (CurrTemp[0] >= SHORT_MAX)
    {
        CurrTemp[0] = SHORT_MAX;
    }
    //
    CurrTemp[1] = ((LONG)(CurrIBus_1 * OffsetNum)) - CurrIBus_Offset;
    if(CurrTemp[1] <= SHORT_MIN)
    {
        CurrTemp[1] = SHORT_MIN;
    }
    else if (CurrTemp[1] >= SHORT_MAX)
    {
        CurrTemp[1] = SHORT_MAX;
    }
    //
    switch(SVPWM.Sector)
    {
        case SECTOR_1:
                switch(SVPWM.TrigType)
                {
                    case TYPE_1:
                        CurrIBus_A = CurrTemp[1];
                        CurrIBus_C = -CurrTemp[0];
                        CurrIBus_B = -(CurrIBus_A + CurrIBus_C);
                        //
                        break;
                        //
                    case TYPE_2:
                        CurrIBus_C = -CurrTemp[1];
                        CurrIBus_A = -CurrTemp[0];
                        CurrIBus_B = -(CurrIBus_A + CurrIBus_C);
                        //
                        break;
                        //
                    case TYPE_3:
                        CurrIBus_A = CurrTemp[1];
                        CurrIBus_B = -CurrTemp[0];
                        CurrIBus_C = -(CurrIBus_A + CurrIBus_B);
                        //
                        break;
                        //
                    case TYPE_4:
                        CurrIBus_A = CurrTemp[1];
                        CurrIBus_B = CurrTemp[0];
                        CurrIBus_C = -(CurrIBus_A + CurrIBus_B);
                        //
                        break;
                        //
                    default:
                        break;
                }
                //
                break;
                //
            case SECTOR_2:
                switch(SVPWM.TrigType)
                {
                    case TYPE_1:
                        CurrIBus_B = CurrTemp[1];
                        CurrIBus_C = -CurrTemp[0];
                        CurrIBus_A = -(CurrIBus_B + CurrIBus_C);
                        //
                        break;
                        //
                    case TYPE_2:
                        CurrIBus_C = -CurrTemp[1];
                        CurrIBus_B = -CurrTemp[0];
                        CurrIBus_A = -(CurrIBus_B + CurrIBus_C);
                        //
                        break;
                        //
                    case TYPE_3:
                        CurrIBus_B = CurrTemp[1];
                        CurrIBus_A = -CurrTemp[0];
                        CurrIBus_C = -(CurrIBus_A + CurrIBus_B);
                        //
                        break;
                        //
                    case TYPE_4:
                        CurrIBus_B = CurrTemp[1];
                        CurrIBus_A = CurrTemp[0];
                        CurrIBus_C = -(CurrIBus_A + CurrIBus_B);
                        //
                        break;
                        //
                    default:
                        break;
                }
                //
                break;
                //
            case SECTOR_3:
                switch(SVPWM.TrigType)
                {
                    case TYPE_1:
                        CurrIBus_B = CurrTemp[1];
                        CurrIBus_A = -CurrTemp[0];
                        CurrIBus_C = -(CurrIBus_A + CurrIBus_B);
                        //
                        break;
                        //
                    case TYPE_2:
                        CurrIBus_A = -CurrTemp[1];
                        CurrIBus_B = -CurrTemp[0];
                        CurrIBus_C = -(CurrIBus_A + CurrIBus_B);
                        //
                        break;
                        //
                    case TYPE_3:
                        CurrIBus_B = CurrTemp[1];
                        CurrIBus_C = -CurrTemp[0];
                        CurrIBus_A = -(CurrIBus_B + CurrIBus_C);
                        //
                        break;
                        //
                    case TYPE_4:
                        CurrIBus_B = CurrTemp[1];
                        CurrIBus_C = CurrTemp[0];
                        CurrIBus_A = -(CurrIBus_B + CurrIBus_C);
                        //
                        break;
                        //
                    default:
                        break;
                }
                //
                break;
                //
            case SECTOR_4:
                switch(SVPWM.TrigType)
                {
                    case TYPE_1:
                        CurrIBus_C = CurrTemp[1];
                        CurrIBus_A = -CurrTemp[0];
                        CurrIBus_B = -(CurrIBus_A + CurrIBus_C);
                        //
                        break;
                        //
                    case TYPE_2:
                        CurrIBus_A = -CurrTemp[1];
                        CurrIBus_C = -CurrTemp[0];
                        CurrIBus_B = -(CurrIBus_A + CurrIBus_C);
                        //
                        break;
                        //
                    case TYPE_3:
                        CurrIBus_C = CurrTemp[1];
                        CurrIBus_B = -CurrTemp[0];
                        CurrIBus_A = -(CurrIBus_B + CurrIBus_C);
                        //
                        break;
                        //
                    case TYPE_4:
                        CurrIBus_C = CurrTemp[1];
                        CurrIBus_B = CurrTemp[0];
                        CurrIBus_A = -(CurrIBus_B + CurrIBus_C);
                        //
                        break;
                        //
                    default:
                        break;
                }
                //
                break;
                //
            case SECTOR_5:
                switch(SVPWM.TrigType)
                {
                    case TYPE_1:
                        CurrIBus_C = CurrTemp[1];
                        CurrIBus_B = -CurrTemp[0];
                        CurrIBus_A = -(CurrIBus_B + CurrIBus_C);
                        //
                        break;
                        //
                    case TYPE_2:
                        CurrIBus_B = -CurrTemp[1];
                        CurrIBus_C = -CurrTemp[0];
                        CurrIBus_A = -(CurrIBus_B + CurrIBus_C);
                        //
                        break;
                        //
                    case TYPE_3:
                        CurrIBus_C = CurrTemp[1];
                        CurrIBus_A = -CurrTemp[0];
                        CurrIBus_B = -(CurrIBus_A + CurrIBus_C);
                        //
                        break;
                        //
                    case TYPE_4:
                        CurrIBus_C = CurrTemp[1];
                        CurrIBus_A = CurrTemp[0];
                        CurrIBus_B = -(CurrIBus_A + CurrIBus_C);
                        //
                        break;
                        //
                    default:
                        break;
                }
                //
                break;
                //
            case SECTOR_6:
                switch(SVPWM.TrigType)
                {
                    case TYPE_1:
                        CurrIBus_A = CurrTemp[1];
                        CurrIBus_B = -CurrTemp[0];
                        CurrIBus_C = -(CurrIBus_A + CurrIBus_B);
                        //
                        break;
                        //
                    case TYPE_2:
                        CurrIBus_B = -CurrTemp[1];
                        CurrIBus_A = -CurrTemp[0];
                        CurrIBus_C = -(CurrIBus_A + CurrIBus_B);
                        //
                        break;
                        //
                    case TYPE_3:
                        CurrIBus_A = CurrTemp[1];
                        CurrIBus_C = -CurrTemp[0];
                        CurrIBus_B = -(CurrIBus_A + CurrIBus_C);
                        //
                        break;
                        //
                    case TYPE_4:
                        CurrIBus_A = CurrTemp[1];
                        CurrIBus_C = CurrTemp[0];
                        CurrIBus_B = -(CurrIBus_A + CurrIBus_C);
                        //
                        break;
                        //
                    default:
                        break;
                }
                //
                break;
                //
            default:
                CurrIBus_A = 0;
                CurrIBus_B = 0;
                CurrIBus_C = 0;
                //
                break;
                //
    }
    //
    CurrOutput.IComp_1 = CurrIBus_A;
    CurrOutput.IComp_2 = CurrIBus_B;
    //
    return(CurrOutput);
}

//*****************************************************************************
//!\brief	Clarke Transform
//!\param	Ia,Ib
//!\retval	Ialpha,Ibeta
//*****************************************************************************
T_CurrComps  Clarke(T_CurrComps CurrInput)
{T_CurrComps CurrOutput; 
LONG Ia_divSQRT3,Ib_divSQRT3;
//
    // Ialpha = Ia
    CurrOutput.IComp_1 = CurrInput.IComp_1;
    // Ibeta = (Ia + 2*Ib)/sqrt(3)
    Ia_divSQRT3 = (LONG)(divSQRT_3 * CurrInput.IComp_1);
    Ia_divSQRT3 /= 32768;
    Ib_divSQRT3 = (LONG)(divSQRT_3 * CurrInput.IComp_2);
    Ib_divSQRT3 /= 32768;
    CurrOutput.IComp_2 = (SHORT)Ia_divSQRT3 + (SHORT)Ib_divSQRT3 + (SHORT)Ib_divSQRT3;
    //
    return(CurrOutput);
}

//*****************************************************************************
//!\brief	Park Transform
//!\param	Ialpha,Ibeta
//!\retval	Id,Iq
//*****************************************************************************
T_CurrComps  Park(T_CurrComps CurrInput,T_TrigComps TrigFunc)
{T_CurrComps CurrOutput; 
LONG Id_tmp_1,Id_tmp_2;
LONG Iq_tmp_1,Iq_tmp_2;
//
    // Id =  Ialpha * cosθ + Ibeta * sinθ
    Id_tmp_1 = (LONG)(CurrInput.IComp_1 * TrigFunc.Cos);
    Id_tmp_1 /= 32768;
    Id_tmp_2 = (LONG)(CurrInput.IComp_2 * TrigFunc.Sin);
    Id_tmp_2 /= 32768;
    CurrOutput.IComp_1 = (SHORT)Id_tmp_1 + (SHORT)Id_tmp_2;
    // Iq = -Ialpha * sinθ + Ibeta * cosθ
    Iq_tmp_1 = (LONG)(-CurrInput.IComp_1 * TrigFunc.Sin);
    Iq_tmp_1 /= 32768;
    Iq_tmp_2 = (LONG)( CurrInput.IComp_2 * TrigFunc.Cos);
    Iq_tmp_2 /= 32768;
    CurrOutput.IComp_2 = (SHORT)Iq_tmp_1 + (SHORT)Iq_tmp_2;
    //
    return(CurrOutput);
}

//*****************************************************************************
//!\brief	Inverse Park Transform
//!\param	Vd,Vq
//!\retval	Valpha,Vbeta
//*****************************************************************************
T_VoltComps  InvePark(T_VoltComps VoltInput,T_TrigComps TrigFunc)
{T_VoltComps VoltOutput;
LONG Valpha_tmp_1,Valpha_tmp_2;
LONG Vbeta_tmp_1,Vbeta_tmp_2;
//
    // Valpha = Vd * cosθ - Vq * sinθ
    Valpha_tmp_1 = (LONG)(VoltInput.VComp_1 * TrigFunc.Cos);
    Valpha_tmp_1 /= 32768;
    Valpha_tmp_2 = (LONG)(-VoltInput.VComp_2 * TrigFunc.Sin);
    Valpha_tmp_2 /= 32768;
    VoltOutput.VComp_1 = (SHORT)Valpha_tmp_1 + (SHORT)Valpha_tmp_2;
    // Vbeta = Vd * sinθ + Vq * cosθ
    Vbeta_tmp_1 = (LONG)(VoltInput.VComp_1 * TrigFunc.Sin);
    Vbeta_tmp_1 /= 32768;
    Vbeta_tmp_2 = (LONG)(VoltInput.VComp_2 * TrigFunc.Cos);
    Vbeta_tmp_2 /= 32768;
    VoltOutput.VComp_2 = (SHORT)Vbeta_tmp_1 + (SHORT)Vbeta_tmp_2;
    //
    return(VoltOutput);
}

//*****************************************************************************
//!\brief  Scale down the vector circle,and limit the size of Vd and Vq
//!\param  Vd,Vq,MODULATION_MAX,Start_Index
//!\retval Vd,Vq
//*****************************************************************************
T_VoltComps  CircleLimit(T_VoltComps VoltInput,uSHORT MaxModule,uCHAR StartIndex)
{T_VoltComps VoltOutput;
LONG  TempData;
uSHORT Index;
static const uSHORT *_pCircleLimit = CircleLimitTable;
//
    TempData = (LONG)(VoltInput.VComp_1 * VoltInput.VComp_1) + (LONG)(VoltInput.VComp_2 * VoltInput.VComp_2);
    //
    if(TempData > (LONG)((MaxModule * MaxModule)))
    {
        TempData /= (LONG)(512*32768);
        TempData -= StartIndex;
        //
        //Index= CircleLimitTable[(uCHAR)TempData];
        Index= *(_pCircleLimit + (uCHAR)TempData);
        //
        TempData = (LONG)(((SHORT)VoltInput.VComp_1) * ((uSHORT)Index));
        VoltOutput.VComp_1 = (SHORT)(TempData/32768);
        // 
        TempData = (LONG)(((SHORT)VoltInput.VComp_2) * ((uSHORT)Index));
        VoltOutput.VComp_2 = (SHORT)(TempData/32768);
    }
    else
    {
        VoltOutput.VComp_1 = VoltInput.VComp_1;
        VoltOutput.VComp_2 = VoltInput.VComp_2;
    }
    //
    return (VoltOutput);
}

//*****************************************************************************
//!\brief	Trigonometric Function
//!\param	Angle in s16 format
//!\retval	cosθ,sinθ
//*****************************************************************************
T_TrigComps  TrigFunc(SHORT Angle)
{uSHORT Index;
T_TrigComps TrigFuncOutput;
static const SHORT *_pSinCos = SinCosTable;
//
    Index = (uSHORT)(Angle + 32768);
    Index /= 64;
    //
    switch (Index & SIN_MASK) 
    {
        case AG0_90:
            //TrigFuncOutput.Sin =  SinCosTable[(uCHAR)(Index)];
            //TrigFuncOutput.Cos =  SinCosTable[(uCHAR)(0xFF-(uCHAR)(Index))];
            //
            TrigFuncOutput.Sin = *(_pSinCos + (uCHAR)(Index));
            TrigFuncOutput.Cos = *(_pSinCos + (uCHAR)(0xFF-(uCHAR)(Index)));
            break;
        case AG90_180:
            //TrigFuncOutput.Sin =  SinCosTable[(uCHAR)(0xFF-(uCHAR)(Index))];
            //TrigFuncOutput.Cos = -SinCosTable[(uCHAR)(Index)];
            //
            TrigFuncOutput.Sin =   *(_pSinCos + (uCHAR)(0xFF-(uCHAR)(Index)));
            TrigFuncOutput.Cos = -(*(_pSinCos + (uCHAR)(Index)));
            break;
        case AG180_270:
            //TrigFuncOutput.Sin = -SinCosTable[(uCHAR)(Index)];
            //TrigFuncOutput.Cos = -SinCosTable[(uCHAR)(0xFF-(uCHAR)(Index))];
            //
            TrigFuncOutput.Sin = -(*(_pSinCos + (uCHAR)(Index)));
            TrigFuncOutput.Cos = -(*(_pSinCos + (uCHAR)(0xFF-(uCHAR)(Index))));
            break;
        case AG270_360:
            //TrigFuncOutput.Sin = -SinCosTable[(uCHAR)(0xFF-(uCHAR)(Index))];
            //TrigFuncOutput.Cos =  SinCosTable[(uCHAR)(Index)]; 
            //
            TrigFuncOutput.Sin = -(*(_pSinCos + (uCHAR)(0xFF-(uCHAR)(Index))));
            TrigFuncOutput.Cos =   *(_pSinCos + (uCHAR)(Index));
            break;
        default:
            break;
    }
    //
    return (TrigFuncOutput);
}

//****************************************************************************
//!\brief	The Space Vector Pulse Width Modulation (SVPWM) Control Logic
//!\param	Valpha,Vbeta,T_PWM,PWMLoad
//!\retval	Sector number
//****************************************************************************
T_SVPWMComps  SVPWM_Ctrl(T_VoltComps VoltInput,uLONG Tpwm,uSHORT Load_PWM,uLONG Tmin,uCHAR TdelayRatio)
{uCHAR  N = 0;
LONG  U1=0;
LONG  U2=0,U2_tmp_1=0,U2_tmp_2=0;
LONG  U3=0,U3_tmp_1=0,U3_tmp_2=0;
uLONG  Tx=0,Ty=0,TMR1_a=0,TMR1_b=0,TMR1_c=0,TMR1_d=0;
uLONG  TMR3_a=0,TMR3_b=0,TMR3_d=0;
//
uLONG  Tx_div2=0,Ty_div2=0,Tmin_div2 = 0;
//
T_SVPWMComps  SVPWM_Output;
//
    U1 = (LONG)VoltInput.VComp_2;
    //
    U2_tmp_1  = (LONG)(VoltInput.VComp_1 * 32768);
    U2_tmp_1 /= (LONG)(divSQRT_3 * 2);
    U2_tmp_2  = (LONG)(-VoltInput.VComp_2 * divSQRT_3);
    U2_tmp_2 /= (LONG)(divSQRT_3 * 2);
    U2 = U2_tmp_1 + U2_tmp_2;
    //
    U3_tmp_1  = (LONG)(-VoltInput.VComp_1 * 32768);
    U3_tmp_1 /= (LONG)(divSQRT_3 * 2);
    U3_tmp_2  = (LONG)(-VoltInput.VComp_2 * divSQRT_3);
    U3_tmp_2 /= (LONG)(divSQRT_3 * 2);
    U3 = U3_tmp_1 + U3_tmp_2;
    //
    if(U1 > 0)
    {
    	N = 1;
    }
    //
    if(U2 > 0) 
    {
    	N += 2;
    }
    //
    if(U3 > 0) 
    {
    	N += 4;
    }
    //
    switch(N)     // N = (A + 2 * B + 4 * C)
    {
    	case 1:
            SVPWM_Output.Sector = SECTOR_2;
            //
            Tx = (((uLONG)(-U2 * Tpwm))/32768);
            Ty = (((uLONG)(-U3 * Tpwm))/32768);
            //
            break;
    	case 2:
            SVPWM_Output.Sector = SECTOR_6;
            //
            Tx = (((uLONG)(-U3 * Tpwm))/32768);
            Ty = (((uLONG)(-U1 * Tpwm))/32768);
            //
            break;
    	case 3:
            SVPWM_Output.Sector = SECTOR_1;
            //
            Tx = (((uLONG)(U2 * Tpwm))/32768);
            Ty = (((uLONG)(U1 * Tpwm))/32768);
            //
            break;
    	case 4:
            SVPWM_Output.Sector = SECTOR_4;
            //
            Tx = (((uLONG)(-U1 * Tpwm))/32768);
            Ty = (((uLONG)(-U2 * Tpwm))/32768);
            //
            break;
    	case 5:
            SVPWM_Output.Sector = SECTOR_3;
            //
            Tx = (((uLONG)(U1 * Tpwm))/32768);
            Ty = (((uLONG)(U3 * Tpwm))/32768);
            //
            break;
    	case 6:
            SVPWM_Output.Sector = SECTOR_5;
            //
            Tx = (((uLONG)(U3 * Tpwm))/32768);
            Ty = (((uLONG)(U2 * Tpwm))/32768);
            //
            break;
    	default:
            SVPWM_Output.Sector = SECTOR_NONE;
            Tx = 0;
            Ty = 0;
            //
    		break;
    }
    //
    if((Tx + Ty) > Tpwm)
    {
        Tx = ((Tx * Tpwm) / (Tx + Ty));
        Ty = ((Ty * Tpwm) / (Tx + Ty));
    }

    //
    Tx_div2   = (Tx >> 1);
    Ty_div2   = (Ty >> 1);
    Tmin_div2 = (Tmin >> 1);
    //
    // PWM_MODE_1
    TMR1_a = ((Tpwm + Tx + Ty) >> 2);
    TMR1_b = (TMR1_a - Tx_div2);
    TMR1_c = (TMR1_b - Ty_div2);

    // PWM_MODE_2
    /*
    TMR1_a = ((Tpwm - Tx - Ty) >> 2);
    TMR1_b = (TMR1_a + Tx_div2);
    TMR1_c = (TMR1_b + Ty_div2);
    */
    //
    if(((Tx_div2 <= Tmin) && (Ty_div2 <= Tmin)) || ((Ty_div2 <= Tmin) && (Tx_div2 <= (Tmin + Tmin_div2))))
    {
        SVPWM_Output.TrigType = TYPE_4;
        //
        TMR1_a += Tmin;
        TMR1_c += Tmin_div2;
        TMR1_d = ((Tmin_div2 * TdelayRatio) / 100);
        //
        TMR3_a = Tmin;
        TMR3_b = Tmin_div2;
        TMR3_d = ((TMR1_a + TMR1_b) >> 1) + ((((TMR1_a - TMR1_b) >> 1) * TdelayRatio) / 100);
    }
    else if(Tx_div2 <= Tmin)
    {
        SVPWM_Output.TrigType = TYPE_2;
        //
        TMR1_a += Tmin_div2;
        TMR1_d = ((Tmin_div2 * TdelayRatio) / 100);
        //
        TMR3_a = Tmin_div2;
        TMR3_b = 0;
        TMR3_d = ((TMR1_b + TMR1_c) >> 1) + ((((TMR1_b - TMR1_c) >> 1) * TdelayRatio) / 100);
    }
    else if(Ty_div2 <= Tmin)
    {
        SVPWM_Output.TrigType = TYPE_3;
        //
        TMR1_b += Tmin_div2;
        TMR1_d = ((Tmin_div2 * TdelayRatio) / 100);
        //
        TMR3_a = Tmin_div2;
        TMR3_b = 0;
        TMR3_d = ((TMR1_a + TMR1_b) >> 1) + ((((TMR1_a - TMR1_b) >> 1) * TdelayRatio) / 100);
    }
    else
    {
        SVPWM_Output.TrigType = TYPE_1;
        //
        TMR1_d = ((TMR1_b + TMR1_c) >> 1) + ((((TMR1_b - TMR1_c) >> 1) * TdelayRatio) / 100);
        //
        TMR3_a = 0;
        TMR3_b = 0;
        TMR3_d = ((TMR1_a + TMR1_b) >> 1) + ((((TMR1_a - TMR1_b) >> 1) * TdelayRatio) / 100);
    }
    // 
    switch(SVPWM_Output.Sector)
    {
        case SECTOR_1:
            SVPWM_Output.TMR3_CC1 = (uSHORT)(((uLONG)(TMR3_a * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR3_CC2 = (uSHORT)(((uLONG)(TMR3_b * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR3_CC4 = (uSHORT)(((uLONG)(TMR3_d * Load_PWM * 2)) / Tpwm);
            //
            SVPWM_Output.TMR1_CC1 = (uSHORT)(((uLONG)(TMR1_a * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR1_CC2 = (uSHORT)(((uLONG)(TMR1_b * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR1_CC3 = (uSHORT)(((uLONG)(TMR1_c * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR1_CC4 = (uSHORT)(((uLONG)(TMR1_d * Load_PWM * 2)) / Tpwm);
            //
            break;
            //
        case SECTOR_2:
            SVPWM_Output.TMR3_CC1 = (uSHORT)(((uLONG)(TMR3_a * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR3_CC2 = (uSHORT)(((uLONG)(TMR3_b * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR3_CC4 = (uSHORT)(((uLONG)(TMR3_d * Load_PWM * 2)) / Tpwm);
            //
            SVPWM_Output.TMR1_CC1 = (uSHORT)(((uLONG)(TMR1_b * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR1_CC2 = (uSHORT)(((uLONG)(TMR1_a * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR1_CC3 = (uSHORT)(((uLONG)(TMR1_c * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR1_CC4 = (uSHORT)(((uLONG)(TMR1_d * Load_PWM * 2)) / Tpwm);
            //
            break;
            //
        case SECTOR_3:
            SVPWM_Output.TMR3_CC1 = (uSHORT)(((uLONG)(TMR3_a * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR3_CC2 = (uSHORT)(((uLONG)(TMR3_b * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR3_CC4 = (uSHORT)(((uLONG)(TMR3_d * Load_PWM * 2)) / Tpwm);
            //
            SVPWM_Output.TMR1_CC1 = (uSHORT)(((uLONG)(TMR1_c * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR1_CC2 = (uSHORT)(((uLONG)(TMR1_a * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR1_CC3 = (uSHORT)(((uLONG)(TMR1_b * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR1_CC4 = (uSHORT)(((uLONG)(TMR1_d * Load_PWM * 2)) / Tpwm);
            //
            break;
            //
        case SECTOR_4:
            SVPWM_Output.TMR3_CC1 = (uSHORT)(((uLONG)(TMR3_a * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR3_CC2 = (uSHORT)(((uLONG)(TMR3_b * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR3_CC4 = (uSHORT)(((uLONG)(TMR3_d * Load_PWM * 2)) / Tpwm);
            //
            SVPWM_Output.TMR1_CC1 = (uSHORT)(((uLONG)(TMR1_c * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR1_CC2 = (uSHORT)(((uLONG)(TMR1_b * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR1_CC3 = (uSHORT)(((uLONG)(TMR1_a * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR1_CC4 = (uSHORT)(((uLONG)(TMR1_d * Load_PWM * 2)) / Tpwm);
            //
            break;
            //
        case SECTOR_5:
            SVPWM_Output.TMR3_CC1 = (uSHORT)(((uLONG)(TMR3_a * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR3_CC2 = (uSHORT)(((uLONG)(TMR3_b * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR3_CC4 = (uSHORT)(((uLONG)(TMR3_d * Load_PWM * 2)) / Tpwm);
            //
            SVPWM_Output.TMR1_CC1 = (uSHORT)(((uLONG)(TMR1_b * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR1_CC2 = (uSHORT)(((uLONG)(TMR1_c * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR1_CC3 = (uSHORT)(((uLONG)(TMR1_a * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR1_CC4 = (uSHORT)(((uLONG)(TMR1_d * Load_PWM * 2)) / Tpwm);
            //
            break;
            //
        case SECTOR_6:
            SVPWM_Output.TMR3_CC1 = (uSHORT)(((uLONG)(TMR3_a * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR3_CC2 = (uSHORT)(((uLONG)(TMR3_b * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR3_CC4 = (uSHORT)(((uLONG)(TMR3_d * Load_PWM * 2)) / Tpwm);
            //
            SVPWM_Output.TMR1_CC1 = (uSHORT)(((uLONG)(TMR1_a * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR1_CC2 = (uSHORT)(((uLONG)(TMR1_c * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR1_CC3 = (uSHORT)(((uLONG)(TMR1_b * Load_PWM * 2)) / Tpwm);
            SVPWM_Output.TMR1_CC4 = (uSHORT)(((uLONG)(TMR1_d * Load_PWM * 2)) / Tpwm);
            //
            break;
            //
        default:
            SVPWM_Output.TrigType = TYPE_NONE;
            //
            SVPWM_Output.TMR3_CC1 = 0;
            SVPWM_Output.TMR3_CC2 = 0;
            SVPWM_Output.TMR3_CC4 = (Load_PWM - 1);
            //
            SVPWM_Output.TMR1_CC1 = 0;
            SVPWM_Output.TMR1_CC2 = 0;
            SVPWM_Output.TMR1_CC3 = 0;
            SVPWM_Output.TMR1_CC4 = 1;
            //
            break;
    }
    //
    TMR3->CC1 = SVPWM_Output.TMR3_CC1;
    TMR3->CC2 = SVPWM_Output.TMR3_CC2;
    if(SVPWM_Output.TMR3_CC4 <= 0 )
    {
        SVPWM_Output.TMR3_CC4 = 1;
    }
    else if(SVPWM_Output.TMR3_CC4 >= Load_PWM)
    {
        SVPWM_Output.TMR3_CC4 = (Load_PWM -1);
    }
    TMR3->CC4 = SVPWM_Output.TMR3_CC4;
    //
    TMR1->CC1 = SVPWM_Output.TMR1_CC1;
    TMR1->CC2 = SVPWM_Output.TMR1_CC2;
    TMR1->CC3 = SVPWM_Output.TMR1_CC3;
    if(SVPWM_Output.TMR1_CC4 <= 0 )
    {
        SVPWM_Output.TMR1_CC4 = 1;
    }
    else if(SVPWM_Output.TMR1_CC4 >= Load_PWM)
    {
        SVPWM_Output.TMR1_CC4 = (Load_PWM -1);
    }
    TMR1->CC4 = SVPWM_Output.TMR1_CC4;
    //
    switch(SVPWM_Output.TrigType)
    {
        case TYPE_1:
            TMR3_INT_CC1_DISABLE;
            TMR3_INT_CC2_DISABLE;
            //
            break;
            //
        case TYPE_2:
        case TYPE_3:
            TMR3_INT_CC1_ENABLE;
            TMR3_INT_CC2_DISABLE;
            g_PWM_InverseIndex = 0;
            //
            break;
            //
        case TYPE_4:
            TMR3_INT_CC1_ENABLE;
            TMR3_INT_CC2_ENABLE;
            g_PWM_InverseIndex = 0;
            //
            break;
            //
        default:
            TMR3_INT_CC1_DISABLE;
            TMR3_INT_CC2_DISABLE;
            //
            break;
    }
    //
    return (SVPWM_Output);
}

//*****************************************************************************
//!\brief   Motor Digital Model
//!\param   CurrInput: the input current
//!\param   VoltInput: the input voltage
//!\param   BEMF: the BEMF after the low pass filtering
//!\param   CorrFactor: the correction factor voltage
//!\param   Gain_F: the gain F of motor digital model
//!\param   Gain_G: the gain G of motor digital model
//!\retval  None
//*****************************************************************************
void MotorDigModel(T_CurrComps32 *CurrInput,T_VoltComps VoltInput,T_VoltComps32 BEMF,LONG CorrFactor_1,LONG CorrFactor_2,SHORT Gain_F_Quo,SHORT Gain_F_Rem,SHORT Gain_G_Quo,SHORT Gain_G_Rem)
{LONG  Data32_tmp_1 = 0,Data32_tmp_2 = 0,Data32_tmp_3 = 0;
//
    Data32_tmp_1 = (LONG)(Gain_F_Rem * (CurrInput->IComp_1));
    Data32_tmp_1 /= 32768;
    Data32_tmp_1 += (LONG)(Gain_F_Quo * (CurrInput->IComp_1));
    //
    Data32_tmp_3 = (LONG)VoltInput.VComp_1 - (LONG)BEMF.VComp_1;
    Data32_tmp_3 = Data32_tmp_3 - (LONG)CorrFactor_1;
    Data32_tmp_2 = (LONG)(Gain_G_Rem * Data32_tmp_3);
    Data32_tmp_2 /= 32768;
    Data32_tmp_2 += (LONG)(Gain_G_Quo * Data32_tmp_3);
    //
    CurrInput->IComp_1 = (Data32_tmp_1 + Data32_tmp_2);
    //
    Data32_tmp_1 = (LONG)(Gain_F_Rem * (CurrInput->IComp_2));
    Data32_tmp_1 /= 32768;
    Data32_tmp_1 += (LONG)(Gain_F_Quo * (CurrInput->IComp_2));
    //
    Data32_tmp_3 = (LONG)VoltInput.VComp_2 - (LONG)BEMF.VComp_2;
    Data32_tmp_3 = Data32_tmp_3 - (LONG)CorrFactor_2;
    Data32_tmp_2 = (LONG)(Gain_G_Rem * Data32_tmp_3);
    Data32_tmp_2 /= 32768;
    Data32_tmp_2 += (LONG)(Gain_G_Quo * Data32_tmp_3);
    //
    CurrInput->IComp_2 = (Data32_tmp_1 + Data32_tmp_2);
}

//*****************************************************************************
//!\brief   Bang-Bang Controller
//!\param   CorrFactor: the correction factor voltage
//!\param   CurrEst: the estimating current
//!\param   CurrInput: the input current
//!\param   Gain_K: the gain K of Bang-Bang Controller
//!\param   ErrorMax: the maximum error of Bang-Bang Controller
//!\retval  None
//*****************************************************************************
void BangBangCtrl(LONG *CorrFactor_1,LONG *CorrFactor_2,T_CurrComps32 CurrEst,T_CurrComps CurrInput,LONG Gain_K,LONG ErrorMax)
{LONG  Error = 0;
//
    Error = CurrEst.IComp_1 - (LONG)CurrInput.IComp_1;
    if((Error > -ErrorMax) && (Error < ErrorMax))
    {
        *CorrFactor_1 = ((Gain_K * Error) / ErrorMax);
    }
    else
    {
         if(Error > 0)
         {
            *CorrFactor_1 = Gain_K;
         }
         else
         {
            *CorrFactor_1 = -Gain_K;
         }
    }
    //
    Error = CurrEst.IComp_2 - (LONG)CurrInput.IComp_2;
    if((Error > -ErrorMax) && (Error < ErrorMax))
    {
        *CorrFactor_2 = ((Gain_K * Error) / ErrorMax);
    }
    else
    {
         if(Error > 0)
         {
            *CorrFactor_2 = Gain_K;
         }
         else
         {
            *CorrFactor_2 = -Gain_K;
         }
    }
}

//*****************************************************************************
//!\brief   Low Pass Filter. y(n) = a*x(n) + (1 - a)*y(n-1) = y(n-1) + a * (x(n) - y(n-1))
//!\param   DataOutput: the output data
//!\param   DataInput: the input data
//!\param   LPFCoeff: Low pass filter coefficient
//!\retval  None
//*****************************************************************************
void LPF16(SHORT *DataOutput,SHORT DataInput,SHORT LPFCoeff)
{LONG  Data32_tmp = 0;
SHORT  Data16_tmp = 0;
//
    Data16_tmp = DataInput - (*DataOutput);
    Data32_tmp = (LONG)(LPFCoeff * Data16_tmp);
    Data32_tmp /= 32768;
    //
    *DataOutput += (SHORT)Data32_tmp;
}

//*****************************************************************************
//!\brief   Low Pass Filter. y(n) = a*x(n) + (1 - a)*y(n-1) = y(n-1) + a * (x(n) - y(n-1))
//!\param   DataOutput: the output data
//!\param   DataInput: the input data
//!\param   LPFCoeff: Low pass filter coefficient
//!\retval  None
//*****************************************************************************
void LPF32(LONG *DataOutput,LONG DataInput,SHORT LPFCoeff)
{LONG  Data32_tmp = 0;
//
    Data32_tmp = DataInput - (*DataOutput);
    Data32_tmp = (LONG)(LPFCoeff * Data32_tmp);
    Data32_tmp /= 32768;
    //
    *DataOutput += Data32_tmp;
}

//*****************************************************************************
//!\brief   Low Pass Filter. y(n) = a*x(n) + (1 - a)*y(n-1) = y(n-1) + a * (x(n) - y(n-1))
//!\param   DataOutput: the output data
//!\param   DataInput: the input data
//!\param   LPFCoeff: Low pass filter coefficient
//!\retval  None
//*****************************************************************************
void LPF32_Double(T_VoltComps32 *DataOutput,LONG DataInput_1,LONG DataInput_2,SHORT LPFCoeff)
{LONG  Data32_tmp = 0;
//
    Data32_tmp = DataInput_1 - (DataOutput->VComp_1);
    Data32_tmp = (LONG)(LPFCoeff * Data32_tmp);
    Data32_tmp /= 32768;
    //
    DataOutput->VComp_1 += Data32_tmp;
    //
    Data32_tmp = DataInput_2 - (DataOutput->VComp_2);
    Data32_tmp = (LONG)(LPFCoeff * Data32_tmp);
    Data32_tmp /= 32768;
    //
    DataOutput->VComp_2 += Data32_tmp;
}

//*****************************************************************************
//!\brief   CORDIC
//!\param   VoltInput: the input data
//!\retval  None
//*****************************************************************************
SHORT  CORDIC(T_VoltComps32 VoltInput)
{SHORT angle_sum = 0,step_x = 0,step_y = 0;
T_VoltComps VoltTemp;
uCHAR  cnt = 0;
static const SHORT *_pTan = TanTable;
// absolute value
    if(VoltInput.VComp_1 < 0)
    {
        VoltTemp.VComp_1 = -VoltInput.VComp_1;
    }
    else
    {
        VoltTemp.VComp_1 = VoltInput.VComp_1;
    }
    //
    if(VoltInput.VComp_2 < 0)
    {
        VoltTemp.VComp_2 = -VoltInput.VComp_2;
    }
    else
    {
        VoltTemp.VComp_2 = VoltInput.VComp_2;
    }
    // CORDIC
    for(cnt= 0;cnt < 10;cnt++)
    {
        step_x = (VoltTemp.VComp_1 >> cnt);
        step_y = (VoltTemp.VComp_2 >> cnt);
        //
        if(VoltTemp.VComp_2 < 0)
        {
            VoltTemp.VComp_1 -= step_y;
            VoltTemp.VComp_2 += step_x;
            //
            //angle_sum -= TanTable[cnt];
            angle_sum -= *(_pTan + cnt);
        }
        else
        {
            VoltTemp.VComp_1 += step_y;
            VoltTemp.VComp_2 -= step_x;
            //
            //angle_sum += TanTable[cnt];
            angle_sum += *(_pTan + cnt);
        }
    }
    // calculate angle
    if(VoltInput.VComp_2 < 0)
    {  
        return ((VoltInput.VComp_1 < 0) ? (-32768 + angle_sum) : (-angle_sum));
    }
    else
    {
        return ((VoltInput.VComp_1 < 0) ? (32767 - angle_sum) : angle_sum);
    }
}

//*****************************************************************************
//!\brief   Adaptive low pass filter coefficient
//!\param   *Coeff: Low pass filter coefficient
//!\param   PWM_Frequency: the pwm frequency
//!\param   PolePairs: the motor polepairs
//!\param   ActulSpeed: the motor actual speed
//!\retval  None
//*****************************************************************************
void SetLPFCoeff(SHORT *Coeff, uLONG PWM_Frequency,uCHAR PolePairs, LONG ActulSpeed)
{uLONG pwm_cnt = 0;
//==============================================================
    pwm_cnt = (uLONG)(60 * PWM_Frequency) / (uLONG)(ActulSpeed * g_MotorPolePairs);
    *Coeff = (SHORT)(205881 / pwm_cnt);                                                     // LPFCoeff = 2*PI/N = (2 * 3.1415926 * 32767) / N = 205881 / N
}

//*****************************************************************************
//!\brief   Adaptive low pass filter coefficient
//!\param   *Coeff:  Low pass filter coefficient
//!\retval  None
//*****************************************************************************
void  AdapLPFCoef(SHORT *Coeff)
{static uLONG pwm_cnt = 0;
//==============================================================
    if(!flag.adp_LPF_coeff_fst)
    {
        flag.adp_LPF_coeff_fst = 1;
        //
        flag.adp_LPF_coeff_en = 0;
        pwm_cnt = 0;
    }
    else
    {
        if(flag.adp_LPF_coeff_en)
        {
            flag.adp_LPF_coeff_en = 0;
            //
            *Coeff = (SHORT)(205881 / pwm_cnt);                                            // LPFCoeff = 2*PI/N = (2 * 3.1415926 * 32767) / N = 205881 / N
            pwm_cnt = 0;
        }
        //
        pwm_cnt++;
    }
}

//*****************************************************************************
//!\brief   Open loop to closed loop control logic
//!\param   *DataOutput: the output data
//!\param   DataInput: the input data
//!\param   CoeffStep: the step of LPF coefficient
//!\retval  None
//*****************************************************************************
T_BOOL  OpenLpToClosedLp(SHORT *DataOutput,SHORT DataInput,SHORT CoeffStep)
{static SHORT adj_coeff = 0;
//
    if(adj_coeff <= 16000)
    {
        adj_coeff += CoeffStep;
    }
    else if(adj_coeff <= 19000)
    {
        adj_coeff += (CoeffStep * 2);
    }
    else if(adj_coeff <= 22000)
    {
        adj_coeff += (CoeffStep * 4);
    }
    else if(adj_coeff <= 25000)
    {
        adj_coeff += (CoeffStep * 8);
    }
    else if(adj_coeff <= 28000)
    {
        adj_coeff += (CoeffStep * 16);
    }
    else
    {
        adj_coeff += (CoeffStep * 32);
    }
    //
    LPF16(DataOutput,DataInput,(SHORT)adj_coeff);
    //
    if(adj_coeff >= 32000)
    {
        adj_coeff = 0;
        //
        return TRUE;
    }
    //
    return FALSE;
}

//========================================================================
//========================== END =========================================
//========================================================================

